function save_data(data_struct, filepath)
%SAVE_DATA 保存数据到MAT文件
%
% 输入:
%   data_struct - 数据结构体
%   filepath - 保存路径

    % 创建目录（如果不存在）
    [folder, ~, ~] = fileparts(filepath);
    if ~exist(folder, 'dir')
        mkdir(folder);
    end
    
    % 保存
    save(filepath, '-struct', 'data_struct', '-v7.3');
    
    fprintf('数据已保存到: %s\n', filepath);
    
    % 同时保存为CSV格式（Python更容易读取）
    [folder, name, ~] = fileparts(filepath);
    csv_path = fullfile(folder, [name '.csv']);
    
    % 转换为表格
    T = length(data_struct.time);
    data_table = table();
    data_table.time = data_struct.time(:);
    data_table.x = data_struct.states(1,:)';
    data_table.y = data_struct.states(2,:)';
    data_table.theta = data_struct.states(3,:)';
    data_table.v = data_struct.states(4,:)';
    
    % 控制量（补齐最后一个时间步）
    controls_padded = [data_struct.controls, data_struct.controls(:,end)];
    data_table.a = controls_padded(1,:)';
    data_table.delta = controls_padded(2,:)';
    
    % 参考轨迹
    data_table.x_ref = data_struct.reference(1,:)';
    data_table.y_ref = data_struct.reference(2,:)';
    data_table.theta_ref = data_struct.reference(3,:)';
    data_table.v_ref = data_struct.reference(4,:)';
    
    writetable(data_table, csv_path);
    fprintf('CSV数据已保存到: %s\n', csv_path);
end